# Untitled - By: wuhuan - 周二 11月 16 2021
from pid import PID
import pyb
import pca9685
import mpu6050
import time
from car import Car
from machine import I2C,Pin
i2c = I2C(sda=Pin('P8'), scl=Pin('P7'))
i2c2=I2C(sda=Pin('P5'), scl=Pin('P4'))
car=Car(i2c,0x40)
mpu=mpu6050.MPU6050(i2c2,0x68)
self_pid = PID(p=24,i=0,d=0)
while(True):
    accel=mpu.get_values()
    self_output = int(self_pid.get_pid(accel,1))
    print(self_output)
    car.run(self_output,self_output)
    print(accel)
    #pyb.delay(30)




